import math
import numpy as np
from PDR.PostureEstimator import PostureEstimator
from PDR.StepEstimator import StepEstimator
#
TYPE_ACC = 1
TYPE_GYR = 2
TYPE_MAG = 3

class mainActicity:
    def __init__(self):
        self.value = [float(0.), float(0.), float(0.)]
        self.value2 = [float(0.), float(0.), float(0.)]
        self.value3 = [float(0.), float(0.), float(0.)]
        self.value4 = [float(0.), float(0.), float(0.)]

        self.step = int(0)
        self.stepLength = float(0.)
        self.stepTime = float(0.)

        self._pe = PostureEstimator()
        self._se = StepEstimator()

    def dataProcess(self, acc_data, gyr_data, mag_data):
        acc_index = 0
        gyr_index = 0
        mag_index = 0

        ACC_INDEX_MAXN = len(acc_data)
        GYR_INDEX_MAXN = len(gyr_data)
        MAG_INDEX_MAXN = len(mag_data)

        result = []

        while True:
            dataType = 0

            if acc_index < ACC_INDEX_MAXN:
                dataType = TYPE_ACC

            if gyr_index < GYR_INDEX_MAXN and (dataType == 0 or (acc_data[acc_index][0] > gyr_data[gyr_index][0] and
                                                                 dataType == TYPE_ACC)):
                dataType = TYPE_GYR

            if mag_index < MAG_INDEX_MAXN and \
                    (dataType == 0 or (dataType == TYPE_ACC and acc_data[acc_index][0] > mag_data[mag_index][0])
                     or (dataType == TYPE_GYR and gyr_data[gyr_index][0] > mag_data[mag_index][0])):
                dataType = TYPE_MAG

            if dataType == 0:
                break

            if dataType == TYPE_ACC:
                self._pe.inputRawData(TYPE_ACC, np.mat(acc_data[acc_index][1:4]), acc_data[acc_index][0])
                postured = self._pe.getPosturedData(np.mat(acc_data[acc_index][1:4]))
                self._pe.getGravityValue()

                self._se.setGravity(self._pe.getGravityValue())
                self._se.inputPosturedData(TYPE_ACC, postured, acc_data[acc_index][0])
                step = self._se.getStepCount()
                stepTime = self._se.getLastStepTime() / 1e9

                direction = self._se.getDirection()
                if direction is None:
                    direction = self._se.getStepDirection()
                    if direction is None:
                        direction = [0., 0.]

                self.value = self._pe.getRawData(np.mat([0, 0, 20]))
                self.value2 = self._pe.getRawData(np.mat([20, 0, 0]))
                self.value3 = self._pe.getRawData(np.mat([0, 20, 0]))
                self.value4 = self._pe.getRawData(np.mat([direction[0], direction[1], 0]))

                #result.append((acc_data[acc_index][0], math.atan2(self.value2[1], self.value2[0]) / math.pi * 180.0))
                result.append((acc_data[acc_index][0], step, self._se.getEstimatedStrideLength(1.8), math.atan2(self.value2[1], self.value2[0]) / math.pi * 180.0))

                acc_index += 1

            if dataType == TYPE_GYR:
                self._pe.inputRawData(TYPE_GYR, np.mat(gyr_data[gyr_index][1:4]), gyr_data[gyr_index][0])
                gyr_index += 1

            if dataType == TYPE_MAG:
                self._pe.inputRawData(TYPE_MAG, np.mat(mag_data[mag_index][1:4]), mag_data[mag_index][0])
                mag_index += 1
        return result







